planar_linkage Derived Type

type, public, extends(kinematic_mechanism) :: planar_linkage

Defines a planar, closed-loop linkage operating within the x-y plane. Each independent loop within the mechanism contributes three loop-closure constraint equations, avoiding the redundant equations that arise when a planar mechanism is analyzed in three dimensions.

All joints must be usable within a planar mechanism, and all link geometry must be arranged such that motion remains within the x-y plane.


Contents

Constructor

Type-Bound Procedures


Constructor

public interface planar_linkage

  • private function pln_init_containers(lnks, jnts, base, effector, tool) result(rst)

    Initializes a new planar_linkage object.

    Arguments

    Type IntentOptional Attributes Name
    type(link_container), intent(in), dimension(:) :: lnks

    The collection of links forming the mechanism.
    type(joint), intent(in), dimension(:) :: jnts

    The collection of joints connecting the links. Every joint must be usable within a planar mechanism.
    integer(kind=int32), intent(in), optional :: base

    The index of the link that is fixed to ground. If not supplied, the first link is used.
    integer(kind=int32), intent(in), optional :: effector

    The index of the link carrying the end-effector. If not supplied, the last link is used.
    real(kind=real64), intent(in), optional :: tool(4,4)

    The transformation matrix relating the end-effector coordinate frame to the body frame of the end-effector link. If not supplied, an identity matrix is used.

    Return Value type(planar_linkage)

    The resulting planar_linkage object.

  • private function pln_init_array(lnks, jnts, base, effector, tool) result(rst)

    Initializes a new planar_linkage object.

    Arguments

    Type IntentOptional Attributes Name
    class(link), intent(in), dimension(:) :: lnks

    The collection of links forming the mechanism.
    type(joint), intent(in), dimension(:) :: jnts

    The collection of joints connecting the links. Every joint must be usable within a planar mechanism.
    integer(kind=int32), intent(in), optional :: base

    The index of the link that is fixed to ground. If not supplied, the first link is used.
    integer(kind=int32), intent(in), optional :: effector

    The index of the link carrying the end-effector. If not supplied, the last link is used.
    real(kind=real64), intent(in), optional :: tool(4,4)

    The transformation matrix relating the end-effector coordinate frame to the body frame of the end-effector link. If not supplied, an identity matrix is used.

    Return Value type(planar_linkage)

    The resulting planar_linkage object.


Type-Bound Procedures

procedure, public :: body_transform => km_body_transform

  • private function km_body_transform(this, i, q) result(rst)

    Computes the transformation matrix relating the body coordinate frame of the requested link to the coordinate frame of the base link. The transformation is accumulated along the path through the mechanism's spanning tree.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.
    integer(kind=int32), intent(in) :: i

    The index of the link of interest (1 = first link).
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing all of the mechanism's joint variables.

    Return Value real(kind=real64), (4,4)

    The resulting 4-by-4 transformation matrix.

procedure, public :: constraint_jacobian => km_constraint_jacobian

  • private function km_constraint_jacobian(this, q) result(rst)

    Computes the Jacobian matrix of the loop-closure constraint equations with respect to the mechanism's joint variables. The derivatives are estimated by means of a central difference approximation.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing all of the mechanism's joint variables.

    Return Value real(kind=real64), allocatable, dimension(:,:)

    The resulting C-by-N matrix where C is the number of constraint equations and N is the number of joint variables.

procedure, public :: constraints => km_constraints

  • private function km_constraints(this, q) result(rst)

    Evaluates the loop-closure constraint equations for the mechanism. A valid configuration drives every equation to zero.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing all of the mechanism's joint variables.

    Return Value real(kind=real64), allocatable, dimension(:)

    An array containing the residual of each constraint equation.

procedure, public :: end_effector_pose => pln_end_effector_pose

  • private function pln_end_effector_pose(this, q) result(rst)

    Computes the planar pose of the end-effector.

    Arguments

    Type IntentOptional Attributes Name
    class(planar_linkage), intent(inout), target :: this

    The planar_linkage object.
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing the actuated joint variables.

    Return Value real(kind=real64), (3)

    A three-element array containing the x and y coordinates of the end-effector along with its orientation angle, in radians.

procedure, public :: end_effector_transform => km_effector_transform

  • private function km_effector_transform(this, q) result(rst)

    Computes the transformation matrix relating the end-effector coordinate frame to the coordinate frame of the base link.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing all of the mechanism's joint variables.

    Return Value real(kind=real64), (4,4)

    The resulting 4-by-4 transformation matrix.

procedure, public :: forward_kinematics => km_forward_kinematics

  • private function km_forward_kinematics(this, q, qo, ib) result(rst)

    Computes the forward kinematics for the mechanism resulting in a transformation matrix relating the end-effector coordinate frame to the coordinate frame of the base link.

    Unlike a serial linkage, the forward kinematics of a closed-loop mechanism require the solution of the loop-closure constraints. The solution obtained depends upon the assembly mode of the mechanism, and therefore upon the starting estimate.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(inout), target :: this

    The mechanism object.
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing the actuated joint variables. The array must contain one value for each degree of freedom of the mechanism.
    real(kind=real64), intent(in), optional, dimension(:) :: qo

    An optional array containing an initial estimate of all of the mechanism's joint variables. If not supplied, the most recently computed configuration is used.
    type(iteration_behavior), intent(out), optional :: ib

    An optional output that can be used to gather information on the solver.

    Return Value real(kind=real64), (4,4)

    The resulting 4-by-4 transformation matrix.

procedure, public :: get_actuated_indices => km_get_actuated_indices

  • private function km_get_actuated_indices(this) result(rst)

    Gets the indices, within the mechanism's array of joint variables, of the actuated joint variables.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value integer(kind=int32), allocatable, dimension(:)

    An array containing the indices of the actuated variables.

procedure, public :: get_actuated_variable_count => km_get_nactuated

  • private pure function km_get_nactuated(this) result(rst)

    Gets the number of actuated joint variables.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value integer(kind=int32)

    The number of actuated joint variables.

procedure, public :: get_actuated_variables => km_get_actuated_variables

  • private function km_get_actuated_variables(this, q) result(rst)

    Extracts the actuated joint variables from a full set of joint variables.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing all of the mechanism's joint variables.

    Return Value real(kind=real64), allocatable, dimension(:)

    An array containing the actuated joint variables.

procedure, public :: get_base_link => km_get_base

  • private pure function km_get_base(this) result(rst)

    Gets the index of the link that is fixed to ground.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value integer(kind=int32)

    The index of the base link.

procedure, public :: get_configuration => km_get_configuration

  • private function km_get_configuration(this) result(rst)

    Gets the most recently computed set of joint variables. This value is used as the starting estimate for subsequent solutions.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value real(kind=real64), allocatable, dimension(:)

    An array containing all of the mechanism's joint variables.

procedure, public :: get_constraint_count => km_get_constraint_count

  • private pure function km_get_constraint_count(this) result(rst)

    Gets the number of loop-closure constraint equations imposed upon the mechanism.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value integer(kind=int32)

    The number of constraint equations.

procedure, public :: get_degrees_of_freedom => km_get_dof

  • private pure function km_get_dof(this) result(rst)

    Gets the number of degrees of freedom, or mobility, of the mechanism. The value is the number of joint variables less the number of loop-closure constraint equations.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value integer(kind=int32)

    The number of degrees of freedom.

procedure, public :: get_end_effector_link => km_get_effector

  • private pure function km_get_effector(this) result(rst)

    Gets the index of the link carrying the end-effector.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value integer(kind=int32)

    The index of the end-effector link.

procedure, public :: get_joint => km_get_joint

  • private function km_get_joint(this, i) result(rst)

    Gets the requested joint object.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.
    integer(kind=int32), intent(in) :: i

    The index of the joint to retrieve (1 = first joint).

    Return Value type(joint)

    The requested joint.

procedure, public :: get_joint_count => km_get_joint_count

  • private pure function km_get_joint_count(this) result(rst)

    Gets the number of joints in the mechanism.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value integer(kind=int32)

    The joint count.

procedure, public :: get_joint_variable_index => km_get_variable_index

  • private function km_get_variable_index(this, i) result(rst)

    Gets the index, within the mechanism's array of joint variables, of the first variable belonging to the requested joint.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.
    integer(kind=int32), intent(in) :: i

    The index of the joint of interest (1 = first joint).

    Return Value integer(kind=int32)

    The index of the joint's first variable.

procedure, public :: get_link => km_get_link

  • private function km_get_link(this, i) result(rst)

    Gets a pointer to the requested link object.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in), target :: this

    The mechanism object.
    integer(kind=int32), intent(in) :: i

    The index of the link to retrieve (1 = first link).

    Return Value class(link), pointer

    A pointer to the requested link.

procedure, public :: get_link_count => km_get_link_count

  • private pure function km_get_link_count(this) result(rst)

    Gets the number of links in the mechanism.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value integer(kind=int32)

    The link count.

procedure, public :: get_loop_count => km_get_loop_count

  • private pure function km_get_loop_count(this) result(rst)

    Gets the number of independent kinematic loops in the mechanism.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value integer(kind=int32)

    The number of independent loops.

procedure, public :: get_space_dimension => pln_get_space_dimension

  • private pure function pln_get_space_dimension(this) result(rst)

    Gets the dimension of the space in which the mechanism operates.

    Arguments

    Type IntentOptional Attributes Name
    class(planar_linkage), intent(in) :: this

    The planar_linkage object.

    Return Value integer(kind=int32)

    The value three, corresponding to two translations and one rotation.

procedure, public :: get_tool_frame => km_get_tool

  • private pure function km_get_tool(this) result(rst)

    Gets the transformation matrix relating the end-effector coordinate frame to the body frame of the end-effector link.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value real(kind=real64), (4,4)

    The 4-by-4 transformation matrix.

procedure, public :: get_variable_count => km_get_variable_count

  • private pure function km_get_variable_count(this) result(rst)

    Gets the total number of joint variables describing the configuration of the mechanism.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.

    Return Value integer(kind=int32)

    The number of joint variables.

procedure, public :: inverse_kinematics => km_inverse_kinematics

  • private function km_inverse_kinematics(this, trg, qo, ib) result(rst)

    Solves the inverse kinematics problem for the mechanism. The loop-closure constraints are solved simultaneously with the end-effector constraints.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(inout), target :: this

    The mechanism object.
    real(kind=real64), intent(in) :: trg(4,4)

    A transformation matrix relating the end-effector coordinate frame to the coordinate frame of the base link. This transformation matrix defines the end-effector target for the solver.
    real(kind=real64), intent(in), optional, dimension(:) :: qo

    An optional array containing an initial estimate of all of the mechanism's joint variables. If not supplied, the most recently computed configuration is used.
    type(iteration_behavior), intent(out), optional :: ib

    An optional output that can be used to gather information on the solver.

    Return Value real(kind=real64), allocatable, dimension(:)

    An array containing the actuated joint variables that satisfy the constraints.

procedure, public :: jacobian => km_jacobian

  • private function km_jacobian(this, q, qo) result(rst)

    Constructs the Jacobian matrix relating the actuated joint velocities to the velocity of the end-effector, as expressed in the end-effector coordinate frame.

    The loop-closure constraints supply the relationship between the actuated and passive joint velocities. Partitioning the constraint Jacobian into its actuated and passive terms yields which is then combined with the end-effector Jacobian. The partition becomes singular at a configuration in which the mechanism loses control of one or more of its degrees of freedom.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(inout), target :: this

    The mechanism object.
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing the actuated joint variables. The array must contain one value for each degree of freedom of the mechanism.
    real(kind=real64), intent(in), optional, dimension(:) :: qo

    An optional array containing an initial estimate of all of the mechanism's joint variables. If not supplied, the most recently computed configuration is used.

    Return Value real(kind=real64), allocatable, dimension(:,:)

    The resulting D-by-M matrix where D is the dimension of the space in which the mechanism operates and M is the number of actuated joint variables.

procedure, public :: joint_transform => km_joint_transform

  • private function km_joint_transform(this, i, q) result(rst)

    Computes the transformation matrix relating the body coordinate frame of a joint's child link to the body coordinate frame of the joint's parent link.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(in) :: this

    The mechanism object.
    integer(kind=int32), intent(in) :: i

    The index of the joint of interest (1 = first joint).
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing all of the mechanism's joint variables.

    Return Value real(kind=real64), (4,4)

    The resulting 4-by-4 transformation matrix.

procedure, public :: pose_error => pln_pose_error

  • private pure function pln_pose_error(this, x) result(rst)

    Reduces a transformation matrix describing the deviation between two coordinate frames into a three-element error vector containing the in-plane translational errors and the rotation about the plane normal.

    Arguments

    Type IntentOptional Attributes Name
    class(planar_linkage), intent(in) :: this

    The planar_linkage object.
    real(kind=real64), intent(in) :: x(4,4)

    The 4-by-4 error transformation matrix.

    Return Value real(kind=real64), allocatable, dimension(:)

    The resulting three-element error vector.

procedure, public :: set_configuration => km_set_configuration

  • private subroutine km_set_configuration(this, q)

    Sets the set of joint variables used as the starting estimate for subsequent solutions. A reasonable estimate is important as a closed-loop mechanism typically admits multiple assembly modes.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(inout) :: this

    The mechanism object.
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing all of the mechanism's joint variables.

procedure, public :: set_tool_frame => km_set_tool

  • private subroutine km_set_tool(this, x)

    Sets the transformation matrix relating the end-effector coordinate frame to the body frame of the end-effector link.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(inout) :: this

    The mechanism object.
    real(kind=real64), intent(in) :: x(4,4)

    The 4-by-4 transformation matrix.

procedure, public :: solve_configuration => km_solve_configuration

  • private function km_solve_configuration(this, q, qo, ib) result(rst)

    Determines the complete set of joint variables satisfying both the loop-closure constraints and the requested actuated joint variables.

    Arguments

    Type IntentOptional Attributes Name
    class(kinematic_mechanism), intent(inout), target :: this

    The mechanism object.
    real(kind=real64), intent(in), dimension(:) :: q

    An array containing the actuated joint variables. The array must contain one value for each degree of freedom of the mechanism.
    real(kind=real64), intent(in), optional, dimension(:) :: qo

    An optional array containing an initial estimate of all of the mechanism's joint variables. If not supplied, the most recently computed configuration is used.
    type(iteration_behavior), intent(out), optional :: ib

    An optional output that can be used to gather information on the solver.

    Return Value real(kind=real64), allocatable, dimension(:)

    An array containing all of the mechanism's joint variables.